#include "StdAfx.h"
#include "SilhouetteTracker.h"


IplImage*       cap_frame = NULL;
IplImage*       im_morphed = NULL;
IplImage*		im_contour = NULL;
IplImage*		im_mask = NULL;

CvCapture*      cap = NULL;
bool			bAbort = false;

// The background subtraction model
CvBGStatModel*	bg_model = NULL;

// Structure Elements for morphological operations
IplConvKernel* seSmallNoise;
IplConvKernel* seConnect;
IplConvKernel* seLarge;

// Storage for contour detection
CvMemStorage* storage = 0;
CvSeq* contours = 0;

// Store file name of last used avi for playback (to support restart)
char* file_playback = NULL;

// Array of last frames, used for temporal smoothing
IplImage* im_smooth[TEMPORAL_SMOOTH_FRAMES];


// Constructor
SilhouetteTracker::SilhouetteTracker(void)
{
    // Initialize capture from camera
	cap = cvCaptureFromCAM(0);
}

	
// Starts the tracking
void SilhouetteTracker::startTracking(void) {

	// Capture initial frame, to check if cam is working
    cap_frame = cvQueryFrame(cap);
    
	if(!cap_frame)
    {	
		printf("[Tracker] Camera / Movie not found!\n");
		exit(0);
    }

	// Initialize parameters of FG-BG separation
	CvFGDStatModelParams params = {CV_BGFG_FGD_LC, CV_BGFG_FGD_N1C, CV_BGFG_FGD_N2C, CV_BGFG_FGD_LCC,
                                CV_BGFG_FGD_N1CC, CV_BGFG_FGD_N2CC, 1, 0, CV_BGFG_FGD_ALPHA_1, CV_BGFG_FGD_ALPHA_2,
                                CV_BGFG_FGD_ALPHA_3, CV_BGFG_FGD_DELTA, CV_BGFG_FGD_T, CV_BGFG_FGD_MINAREA};


	params.minArea = 50;
	params.is_obj_without_holes = 1;
	params.alpha2 = 0.0001f;
	params.T = 0.2f;

	// Create model
	cvFlip(cap_frame, cap_frame,1);
	bg_model = cvCreateFGDStatModel( cap_frame, &params);

    // Create windows (mostly for debug use...)
	cvNamedWindow("BG", 1);
    cvNamedWindow("FG", 1);
	cvNamedWindow("MORPH", 1);

	// Initialize further image buffers
	im_morphed = cvCloneImage(bg_model->foreground);
	im_contour = cvCloneImage(bg_model->foreground);
	im_mask = cvCloneImage(bg_model->foreground);

	// Initialize structuring element for morphological operations
	int n = 1;
	seSmallNoise = cvCreateStructuringElementEx( n*2+1, n*2+1, n, n, CV_SHAPE_ELLIPSE, 0 );
	n = 10;
	seConnect = cvCreateStructuringElementEx( n*2+1, n*2+1, n, n, CV_SHAPE_ELLIPSE, 0 );
	n = 10;
	seLarge = cvCreateStructuringElementEx( n*2+1, n*2+1, n, n, CV_SHAPE_ELLIPSE, 0 );

	// Setup buffers for contour tracking
	storage = cvCreateMemStorage(0);
 
	// Setup buffers for temporal smoothing
	for (int i = 0; i < TEMPORAL_SMOOTH_FRAMES; i++) {
		im_smooth[i] = cvCloneImage(im_mask);
	}
}


// Aquire one frame and do tracking
void SilhouetteTracker::DoTrack(void) {
    // Read new frame
	cap_frame = cvQueryFrame(cap);

	// Check if frame is valid
	if (cap_frame == NULL) {
		// If playback, try to restart
		if (file_playback != NULL) {
			UseRawAVIInstead(file_playback);
			cap_frame = cvQueryFrame(cap);
		} else {
			bAbort = true;
			return;
		}
	}
	
	cvFlip(cap_frame, cap_frame,1);

    // Update model (i.e. feed latest capture into the model
	cvUpdateBGStatModel( cap_frame, bg_model );
    cvShowImage("BG", bg_model->background);
	
	// Ensure origin is the same with both images (prevents images from being flipped
	bg_model->foreground->origin = bg_model->background->origin;
    cvShowImage("FG", bg_model->foreground);

	// Perform OpenClose
	
	// Remove small noise
	cvErode(bg_model->foreground,im_morphed,seSmallNoise,1);

	// Connect blops
    //cvDilate(im_morphed,im_morphed,seConnect,1);

	// Find largest contour
	cvThreshold(im_morphed, im_contour, 0.5, 255, CV_THRESH_BINARY );
	

	// Code stolen from Monzy :)

	CvMemStorage *storage = cvCreateMemStorage(0);
	CvSeq *sampleContours = NULL;
	cvZero(im_mask);

	cvFindContours(im_contour, storage, &sampleContours, sizeof(CvContour), CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);

	for (CvSeq *contour = sampleContours; contour != NULL; contour = contour->h_next) {
	  if ( (fabs(cvArcLength(contour)) > MIN_LENGTH) && (fabs(cvContourArea(contour)) > MIN_AREA) ) {
		cvDrawContours(im_mask, contour, cvScalar(0xFF), cvScalar(0xFF), 0, CV_FILLED, 8);
	  }
	}

	cvReleaseMemStorage(&storage);

	// Do temporal filtering
	/*
	for (int x=0; x < im_mask->width; x++) {
		for (int y=0; y < im_mask->height; y++) {
			// Calculate current position
			int pos = x + y*im_mask->widthStep;
			
			if (image->imageData[pos] == 0){

		}
	}

	for (int i = 0; i < TEMPORAL_SMOOTH_FRAMES; i++) {
		im_smooth[i] = cvCloneImage(im_mask);
	}
	*/

	// Show
	im_mask->origin = bg_model->background->origin;
	cvShowImage("MORPH", im_mask);

	// Check if user hit Esc
	char c = cvWaitKey(1);
	if (c == 27)
		bAbort = true;

}

// Get current foreground
IplImage* SilhouetteTracker::GetForeground()
{
	return im_mask;
}

// Get current raw image
IplImage* SilhouetteTracker::GetRaw()
{
	return cap_frame;
}

// Returns if the user wants to quit
bool SilhouetteTracker::Abort(void) {
	return bAbort;
}

// Load's a avi instead of a cam as video source
void SilhouetteTracker::UseRawAVIInstead(char* file) {
	// Store filename to support restart

	cap = cvCaptureFromFile(file);
	file_playback = (char*) malloc(sizeof(file));
	strcpy(file_playback, file);
}

// Recalibrates the background subtraction
void SilhouetteTracker::recalibrateTracking(void) {
	//int orient = bg_model->background->origin;
	cap_frame = cvQueryFrame(cap);	
	bg_model->background = cvCloneImage(cap_frame);
	cvFlip(bg_model->background, bg_model->background,1);
	
}


SilhouetteTracker::~SilhouetteTracker(void){
	// Clean up
	cvReleaseStructuringElement(&seLarge);
	cvReleaseStructuringElement(&seSmallNoise);
	cvReleaseStructuringElement(&seConnect);
    cvReleaseBGStatModel( &bg_model );
	// responsible for some sort of crash at the end: cvReleaseImage(&cap_frame);
    cvReleaseCapture(&cap);
	
	cvReleaseImage(&im_morphed);


}